/* Copyright 2001,2002,2003 NAH6
 * All Rights Reserved
 *
 * Parts Copyright DoD, Parts Copyright Starium
 *
 */
#include "main.h"
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include "lp_anal.h"
#include "pctolsf.h"
#include "bwexp.h"
#include "celp_sup.h"
#include "code_lsf.h"
#include "setarray.h"

static void ApplyWindow(
float	SpeechIn[F_LEN],
float	HamSpeech[F_LEN]);

static void AutoCor(
float	speech_in[F_LEN], 
float	ac[ORDER+1]);

static void ACtoPC(
float	ac[ORDER+1],
float	pc[ORDER+1]);

static void ACtoRC(
float ac[ORDER+1],
float rc[ORDER],
float *err);

static void HighFreqCorrect(
float	ac[ORDER+1],
float 	alpha);

static void RCtoPC(
float 	k[ORDER], 
float	pc[ORDER]);

static void Durbin(
float	ac[], 
float	pc[]);

/**************************************************************************
*                                                                         *
* ROUTINE
*		LP_Analysis
*
* FUNCTION
*		Linear Predictive analysis
* SYNOPSIS
*		LP_Analysis(SpeechIn, qlsf, frame_num)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*	SpeechIn	float	 i	Frame of input speech
*	qlsf		float	 o	Frame of quantized LSFs
*	frame_num	int	 i	CELP frame number
*
**************************************************************************
*
* DESCRIPTION
*
*	This performs an autocorrelation LP analysis of speech
*	returning a set of line spectral frequencies representing
*	the spectrum of the input speech signal.
*
**************************************************************************
*
* CALLED BY
*
*	Analysis
*
* CALLS
*
*	ApplyWindow AutoCor ACtoPC BWexp_PCs PCtoLSF QuanLSF 
*
*
**************************************************************************/

#define 	TRUE	1
#define		FALSE	0

void LP_Analysis(
float 	SpeechIn[F_LEN],
float 	qlsf[ORDER],
int	frame_num)
{
float	lsf[ORDER+1];
float	ac[ORDER+1], pc[ORDER+1], pcexp[ORDER+1];
float 	HamSpeech[F_LEN];
int	findex[ORDER];


/*  Apply Hamming window to Input Speech */
	ApplyWindow(SpeechIn, HamSpeech);

/*  Calculate Autocorrelations */
	AutoCor(HamSpeech, ac);

/*  Convert ACs to PCs */
	ACtoPC(ac, pc);

/*  Bandwidth Expand PCs */
	BWExpand(OMEGA, pc, pcexp);

/*  Convert PCs to LSFs */
	PCtoLSF(pcexp, lsf);

/*  Quantize the LSFs */
	QuanLSF(lsf, qlsf, findex);

}


/*

*************************************************************************
*                                                                         *
* ROUTINE
*		ApplyWindow
*
* FUNCTION
*		Apply a Hamming Window to input speech
* SYNOPSIS
*		ApplyWindow(SpeechIn, HamSpeech)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*	SpeechIn	float	  i	Frame of input speech
*	HamSpeech	float	  o	Frame of output speech
*
**************************************************************************/

void ApplyWindow(
float	SpeechIn[F_LEN],
float	HamSpeech[F_LEN])
{
static int 	first=TRUE;
static float	HamWindow[F_LEN];
int 		i;

/* Generate Hamming window */
	if(first)	{
	  first=FALSE;
	  CreateHam(HamWindow, F_LEN);
	}


/* Apply Hamming window to input speech */
	for(i=0;i<F_LEN;i++)	{
	  HamSpeech[i] = SpeechIn[i] * HamWindow[i];
	}

}


/*

*************************************************************************
*                                                                         *
* ROUTINE
*		AutoCor
*
* FUNCTION
*		Calculate the autocorrelations for a frame of speech
* SYNOPSIS
*		AutoCor(speech_in, ac)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*	speech_in	float	  i	Frame of input speech
*	ac		float	  o	Autocorrelations
*
**************************************************************************/

void AutoCor(
float	speech_in[F_LEN], 
float	ac[ORDER+1])
{
int i, j;

/*  Calculate autocorrelations */
	for (i=0; i<ORDER+1; i++)
	{
	   for (ac[i]=0, j=i; j<F_LEN; j++)
	      ac[i]+=speech_in[j]*speech_in[j-i];
	}
}

/*

*************************************************************************
*                                                                         *
* ROUTINE
*		ACtoPC
*
* FUNCTION
*		Calculate the Predictor Coefficients for a frame of speech
* SYNOPSIS
*		ACtoPC(ac, pc)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*	ac		float	 i	Autocorrelations
*	pc		float	 o	Predictor Coefficients
*
**************************************************************************/

void ACtoPC(
float	ac[ORDER+1],
float	pc[ORDER+1])
{
float	alpha;
float	rc[ORDER];

#ifdef DURBIN
/*  Use Durbin recursion to convert from ACs to PCs */
	SetArray(ORDER+1, 0.0, pc);
	pc[0] = 1.0;
	Durbin(ac, pc);
#else
/* Convert ACs to RCs */
	ACtoRC(ac, rc, &alpha);

/*  Perform high frequency correction */
	HighFreqCorrect(ac, alpha);

/*  Convert corrected ACs to RCs */
	ACtoRC(ac, rc, &alpha);

/*  Convert corrected RCs to PCs */
	RCtoPC(rc, pc);
#endif
}

/*

****************************************************************************
*
* NAME
*       Durbin
*
*
* SYNOPSIS
*
*       subroutine Durbin (ac, pc)
*
*   formal 
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*	ac[ORDER]	real	i	auto-correlation coefficients
*       pc[ORDER+1]     real    o       prediction coeffs (voiced-> +pc[1])
*       
****************************************************************************
*       
* DESCRIPTION
*
*       This performs the classical Durbin recursion
*       on the correlation sequence c[0], c[1], c[2] . . .
*       to obtain n reflection coefficients (rc).
*
*	The sign convention used defines the first reflection coefficient
*	as the normalized first autocorrelation coefficient, which results
*	in positive values of rc[0] for voiced speech.
*
****************************************************************************
*
* CALLED BY
*
*	ACtoPC
*
* CALLS
*
*	SetArray 
*
*
****************************************************************************     *
* REFERENCES
*
*	Parsons, Voice and Speech Processing, McGraw-Hill, 1987, p.160&378.
*
****************************************************************************/

void Durbin(
float	ac[], 
float	pc[])
{
  int i, j;
  float alpha, beta, rc[ORDER], tmp[ORDER];

  /* If zero energy, set rc's to zero & return  */

  if (ac[0] <= 0.0) 
  {
    for (i = 0; i < ORDER; i++)
      rc[i] = 0.0;
    return;
  }

  /* Intialization   */

  alpha = ac[0];
  pc[0] = rc[0] = -ac[1] / ac[0];
  beta = ac[1];

  /* Recursion   */

  for (i = 1; i < ORDER; i++)
  {
     alpha = alpha + beta * rc[i - 1];
     beta = ac[i+1];

     for (j = 0; j <= i - 1; j++)
       beta = beta + ac[j+1] * pc[i-j-1];
     rc[i] = -beta / alpha;

     for (j = 0; j <= i - 1; j++)
       tmp[j] = rc[i] * pc[i-j-1];

     for (j = 0; j <= i - 1; j++)
       pc[j] = pc[j] + tmp[j];
     pc[i] = rc[i];
   }
   /* Rearrange array ordering and set pc[0]=1 */
   for (i = ORDER /* !!OUTOFBOUNDS!! +1 */; i > 0; i--)
     pc[i] = pc[i-1];
   pc[0] = 1;
}       


/*
**************************************************************************
*
* NAME
*	ACtoRC 
*
* FUNCTION
*
*	Schur recursion to do autocorrelation analysis.
*	Converts autocorrelation sequence to reflection coefficients.
*
* SYNOPSIS
*
*	subroutine ACtoRC (ac, rc, err)
*
*   formal 
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*	ac(ORDER+1)	float	i	auto-correlation coefficients
*       rc(ORDER)	float   o       reflection coefficients (voiced->+rc1)
*	err		float	i/o	normalized prediction error
*       
***************************************************************************
*       
* DESCRIPTION
*
*       This performs the classical Schur recursion (rediscovered by
*	LeRoux & Guegen) on the correlation sequence ac(0), ac(1), ac(2)...
*	to obtain n reflection coefficients (rc).  The recursion can be
*	performed entirely in fractional arithemetic because the
*	correlation sequence is normalized by ac(0), so all the quantities
*	in the recursion are less than unity magnitude (except ac(0)).
*
*	The sign convention used defines the first reflection coefficient
*	as the normalized first autocorrelation coefficient, which results
*	in positive values of rc(1) for voiced speech.
*
***************************************************************************
*
* CALLED BY
*
*	ACtoPC
*
* CALLS
*
*	SetArray
*
***************************************************************************
*       
* REFERENCES
*
*	Parsons, Voice and Speech Processing, McGraw-hill, 1987, p.160&378.
*
***************************************************************************
*
* PROCESS DESCRIPTION
*
*       Name    Type    Function
*
*       d       float      Recursion sequence (dim n)
*       g       float      Recursion sequence (dim n)
*       p       int        Orderindex recursion
*       err     float      Backward error
*       err0    float      Backward error of order 0
*
**************************************************************************/

void ACtoRC(
float ac[ORDER+1],
float rc[ORDER],
float *err)
{
  float *d, *g, rch;
  int i, p;

  d=(float *)calloc(ORDER, sizeof(float));
  if(d == NULL)	{
    printf("Error callocating memory (d) in actorc\n");
    exit(1);
  }

  g=(float *)calloc(ORDER, sizeof(float));
  if(g == NULL)	{
    printf("Error callocating memory (g) in actorc\n");
    exit(1);
  }

/* If zero energy, set rc's to zero & return	*/
  if (ac[0] <= 0.0)   {
    SetArray(ORDER, 0.0, rc);
    return;
  }
  for (i = 0; i < ORDER; i++)
    g[i] = d[i] = ac[i+1] / ac[0];
  rch = g[0];
  rc[0] = rch;
  *err = 1. - rch * rch;
  for (p = 1; p < ORDER; p++)
  {
    for (i = 0; i < ORDER - p; i++)
    {
      g[i] = g[i+1] - rch * d[i];
      d[i] = d[i] - rch * g[i+1];
    }

/* Extract the reflection coefficient	*/
    rch = g[0] / *err;		
    rc[p] = rch;

/* The mean squares error recursion */
    *err = *err * (1. - rch * rch); 
  }
  free(d);
  free(g);
}

/*

*************************************************************************
*                                                                         *
* ROUTINE
*		HighFreqCorrect
*
* FUNCTION
*		Perform high frequency correction on input 
*		(eq. 16, Atal & Schroeder)
* SYNOPSIS
*		HighFreqCorrect(ac, alpha)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*	ac		float	  i	Frame of autocorrelation coefficients
*	alpha		float	  i	Normalized prediction error
*
**************************************************************************/
void HighFreqCorrect(
float	ac[ORDER+1],
float 	alpha)
{
float	lemin;

/*  Unnormalized prediction error scaled by lambda */
	lemin = LAMBDA * ac[0] * alpha;
	
/* High Frequency Correction  */
	ac[0]  += 0.375  * lemin;
	ac[1] -= 0.25   * lemin;
	ac[2] += 0.0625 * lemin;
}

/*

**************************************************************************
*
* NAME
*	RCtoPC
*
* FUNCTION
*
*	convert reflection coefficients into lpc coefficients
*
*	BEWARE:  This code does not use memory efficiently.
*
* SYNOPSIS
*
*	subroutine RCtoPC(k, pc)
*
*   formal 
*                       data	I/O
*	name		type	type	function
*	-------------------------------------------------------------------
*	k		float	i	reflection coefficients (m)
*	pc		float	o	predictor parameters (m+1: a(1)=1.0)
*
***************************************************************************
*	
* DESCRIPTION
*
*	Converts reflection coefficients into lpc coefficients.
*
*	CELP's LP predictor coefficient convention is:
*              p+1         -(i-1)
*       A(z) = SUM   a   z          where a  = +1.0
*              i=1    i                    1
*
*
***************************************************************************
*
* CALLED BY
*
*	ACtoPC
*
***************************************************************************
*	
* REFERENCES
*
*	Rabiner & Schafer, Digital Processing of Speech Signals,
*	Prentice-Hall, 1978, p. 443, equations 8.131a&b&c.
*
**************************************************************************/

void RCtoPC(
float 	k[ORDER], 
float	pc[ORDER])
{
  int i, j;
  float a[ORDER+1][ORDER+1];

/* intialize the array						 */
  for (i = 0; i <= ORDER; i++)
  {
    for (j = 0; j <= ORDER; j++)
    {
      a[j][i] = 0.0;
    }
  }

/* convert reflection coefficients				*/
  for (i = 1; i <= ORDER; i++)
  {
    a[i][i] = k[i-1];
    for (j = 1; j <= i-1; j++)
      a[j][i] = a[j][i-1] - k[i-1] * a[i-j][i-1];
  }
  pc[0] = 1.0;
  for (j = 1; j <= ORDER; j++)
    pc[j] = -a[j][ORDER];
}
